/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_

#include <costmap_2d/cost_values.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
#include <geometry_msgs/PoseStamped.h>

namespace costmap_2d
{
class Layer;

/** LayeredCostmap类是Costmap2DROS的成员，含有主地图，并能通过它操作各层子地图。对各个地图层轮流处理完后，实现地图的更新。
* 这里关注一下两个被Costmap2DROS调用的函数: 地图尺寸设置resizeMap()/地图更新updateMap()
* @class LayeredCostmap
* @brief Instantiates different layer plugins and aggregates them into one score
* 1.调整地图尺寸resizeMap
* 2.更新地图updateMap
* 3.显示当前状态
* 4.设置footprint
*/
class LayeredCostmap
{
public:
    /**
     * @brief  Constructor for a costmap
     */
    LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);

    /**
     * @brief  Destructor
     */
    ~LayeredCostmap();

    /**
     * @brief  Update the underlying costmap with new data.
     * If you want to update the map outside of the update loop that runs, you can call this.
     */
    void updateMap(double robot_x, double robot_y, double robot_yaw);

    inline const std::string &getGlobalFrameID() const noexcept { return global_frame_; }

    void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
                   bool size_locked = false);

    void getUpdatedBounds(double &minx, double &miny, double &maxx, double &maxy)
    {
        minx = minx_;
        miny = miny_;
        maxx = maxx_;
        maxy = maxy_;
    }

    bool isCurrent();

    Costmap2D *getCostmap() { return &costmap_; }

    bool isRolling() { return rolling_window_; }

    bool isTrackingUnknown()
    {
#ifdef SEMANTIC
        return toXJUcost(costmap_.getDefaultValue()) == costmap_2d::XJU_COST_NO_INFORMATION;
#else
        return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
#endif
    }

    std::vector<boost::shared_ptr<Layer>> *getPlugins() { return &plugins_; }

    void addPlugin(boost::shared_ptr<Layer> plugin) { plugins_.push_back(plugin); }

    bool isSizeLocked() { return size_locked_; }

    void getBounds(unsigned int *x0, unsigned int *xn, unsigned int *y0, unsigned int *yn)
    {
        *x0 = bx0_;
        *xn = bxn_;
        *y0 = by0_;
        *yn = byn_;
    }

    bool isInitialized() { return initialized_; }

    /** @brief Updates the stored footprint, updates the circumscribed
     * and inscribed radii, and calls onFootprintChanged() in all
     * layers. */
    void setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec);

    /** @brief Returns the latest footprint stored with setFootprint(). */
    const std::vector<geometry_msgs::Point> &getFootprint() { return footprint_; }

    /** @brief 以机器人原点为中心的圆的半径，它围绕着机器人足迹上的所有点。
     * The radius of a circle centered at the origin of the robot which just surrounds all points on the robot's footprint.
     * This is updated by setFootprint(). */
    double getCircumscribedRadius() { return circumscribed_radius_; }

    /** @brief 以机器人原点为圆心的圆的半径，它正好在机器人足迹的所有点和边缘内。
     * The radius of a circle centered at the origin of the robot which is just within all points and edges of the robot's footprint.
     * This is updated by setFootprint().
     */
    double getInscribedRadius() { return inscribed_radius_; }

#ifdef SEMANTIC
    unsigned char interpretValue(unsigned char old_grid);
#endif

    std::deque<geometry_msgs::PoseStamped> RobotPoseDeque;
private:
    Costmap2D costmap_;
#ifdef SEMANTIC
    Costmap2D semantic_costmap_ = Costmap2D();
#endif
    std::string global_frame_; // global_costmap时为map, local_costmap时为odom_combined
    // Whether or not the costmap should roll with the robot 代价地图是否应该随机器人滚动
    bool rolling_window_;
    bool current_;
    double minx_, miny_, maxx_, maxy_;
    unsigned int bx0_, bxn_, by0_, byn_;

    std::vector<boost::shared_ptr<Layer>> plugins_;
    bool initialized_;
    bool size_locked_;
    // 机器人外接圆和内切圆半径
    double circumscribed_radius_, inscribed_radius_;
    // 机器人足迹(模型)参数
    std::vector<geometry_msgs::Point> footprint_;
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_LAYERED_COSTMAP_H_
